

#include "xmodule_config.h"

#if MODULE_CHECK_ENABLED( ACCELERATOR ) && KX022_CONFIG_ENABLE
#include <string.h>

#define KX022_SLAVE_ADDR       KX022_SLAVE_ADDR_7BIT

// shared variable 
// extern em_accel_data_array_t m_acc_data; 
// extern uint8_t g_sensor_mode ;
// extern uint8_t axis_data_sleep[BYTES_OF_3_AXISES];
// extern uint8_t axis_data_diff_count;
// extern uint8_t axis_data_same_count; 


static const uint8_t INIT_ARRAY[][2] = 
{
    { 0x18, 0x00 }, // CNTL1, stop 
    { 0x1B, 0x02 }, // sample freq: 50Hz
    { 0x3B, 0xC1 }, // buff active, 16bit, stream mode 
    { 0x1C, 0x20 }, // INT1 enabled, active low, latch 
    { 0x1A, 0x03 }, // wakeup freq: 12Hz
    { 0x1D, 0x3F }, // all direction wakeup enabled 
    { 0x23, 0x00 }, // wakeup time 
    { 0x30, 0x01 }, // wakeup threshold 
    { 0x1F, 0x02 }, // wakeup interrupt on INT1
    { 0x18, 0x00 }  // CNTL1, stop again
};

//////////////////////////////////////////////////////////////////////////////
static bool  kx022_read_byte(uint8_t reg, uint8_t* data)
{
    uint32_t ret =  ACCEL_REPEAT_READ( KX022_SLAVE_ADDR, reg, data, 1 ) ;
    if( ret != ACCEL_BUS_SUCCESS )
    {
        LOG1("[KX022]: read err 0x%X", ret );
    }
    return ret == ACCEL_BUS_SUCCESS;
}
static bool  kx022_write_byte(uint8_t reg, uint8_t data)
{
    accel_op_data_t write;
    write.reg = reg;
    write.data[0] = data;

    uint32_t ret = ACCEL_WRITE( KX022_SLAVE_ADDR, &write.reg, write.data, 1 );
    if( ret != ACCEL_BUS_SUCCESS )
    {
        LOG1("[KX022]: write err 0x%X", ret );
    }
    
    return ret == ACCEL_BUS_SUCCESS; 
}
static bool  kx022_read_bytes(uint8_t reg, uint8_t* pdata, uint16_t len)
{
    return ACCEL_REPEAT_READ( KX022_SLAVE_ADDR, reg, pdata, len ) == ACCEL_BUS_SUCCESS;
}


static
void kx022_setup(void)
{        
    int i ;
    uint8_t cotr = 0;
    for( i = 0; i < 10; i++)
    {
         kx022_write_byte( 0x7F, 0x00 );
         kx022_write_byte( 0x19, 0x00 );
         kx022_write_byte( 0x19, 0x00 );
        ACCEL_OS_DELAY( 3 );
        
        if(  kx022_read_byte( 0x0C, &cotr) && cotr == 0x55 ) 
        {
            break;
        }
        ACCEL_OS_DELAY( 10 );
    }
    
    if( cotr == 0x55 ) 
    {
        for( i = 0; i < sizeof( INIT_ARRAY ) / sizeof(INIT_ARRAY[0]); i ++ )
        {
            if( ! kx022_write_byte( INIT_ARRAY[i][0], INIT_ARRAY[i][1] ) )
            {
                // print error 
                break; 
            }
        }
    }
}

static
bool kx022_mounting(em_accel_desc_t* pdesc)
{
    ACCEL_SET_SLAVE_ADDR( KX022_SLAVE_ADDR );
    
    int i ;
    for( i = 0; i < KX022_MOUNTING_TRY_TIMES; i ++ ) 
    {
        if(  kx022_read_byte( 0x0f, &pdesc->chip_id ) && pdesc->chip_id == 0x14) 
        {
            kx022_setup();
            return true;
        }
        ACCEL_OS_DELAY( 10 );
    }
    
    return false;
}

static
void kx022_read_axis( int16_t* data )
{    
    ACCEL_SET_SLAVE_ADDR( KX022_SLAVE_ADDR );
    kx022_read_bytes( 0x06, (uint8_t*)data, 6 );

    data[0] /= 8;
    data[1] /= 8;
    data[2] /= 8;
}

static
const em_accel_data_array_t* kx022_get_fifo(void)
{    
    ACCEL_SET_SLAVE_ADDR( KX022_SLAVE_ADDR );
    
    uint8_t buf_count;
    if( ! kx022_read_byte( 0x3C, &buf_count ))
    {
        return NULL;
    }
    m_acc_data.has_remain = false;
    if( buf_count > ACCEL_FIFO_BUFF_BYTES )
    {
        buf_count = ACCEL_FIFO_BUFF_BYTES ;
        m_acc_data.has_remain = true;
    }
    
    m_acc_data.nb = buf_count;
    m_acc_data.nb_of_fifo = buf_count / BYTES_OF_3_AXISES; 
    if( buf_count > 0)
    {
         kx022_read_bytes( 0x3F, (uint8_t*)m_acc_data.data, buf_count ); // read FIFO buffer 
    }
        
    return &m_acc_data;
}

// static
// void em_accelerator_sleep_check(void)
// {
//     ACCEL_SET_SLAVE_ADDR( KX022_SLAVE_ADDR );
    
//     bool restart = true;
//     uint8_t data[6];
//      kx022_read_bytes( 0x06, data, 6 );

//     if( memcmp( data, axis_data_sleep, 6) != 0)
//     {
//         axis_data_diff_count ++;
//         axis_data_same_count = 0;
//         if( axis_data_diff_count > 5)
//         {
//             restart = false;
//         }
//     }
//     else 
//     {
//         axis_data_diff_count = 0;
//         axis_data_same_count ++;
//         if( axis_data_same_count >= 10)
//         {
//             axis_data_same_count = 0;
//              kx022_write_byte( 0x18, 0x00 ); 
//              kx022_write_byte( 0x18, 0x82 );
//         }
//     }
//     memcpy( axis_data_sleep, data, 6 );
//     if( restart )
//     {
//         accel_timer_start();
//     }
// }

static 
void kx022_normal_mode(void)
{
    ACCEL_SET_SLAVE_ADDR( KX022_SLAVE_ADDR );
    
    uint8_t dummy;
     kx022_write_byte( 0x18, 0x00 );

     kx022_read_byte( 0x17, &dummy );  // clear interrupt 
     kx022_write_byte( 0x1B, 0x02 );   // ODCNTL set sample rate = 50Hz
     kx022_write_byte( 0x3E, 0xFF );   // BUF_CLEAR clear FIFO
     kx022_write_byte( 0x1D, 0x00 );   // INC2 disable all direction
     kx022_write_byte( 0x1A, 0x00 );   // CNTL3
     kx022_write_byte( 0x1F, 0x20 );   // INC4 watermark interrupt on INT1
     kx022_write_byte( 0x3A, 0x09 );   // BUF_CNTL1 200ms active
     kx022_write_byte( 0x3B, 0xC1 );   // BUF_CNTL2 sample buffer active

     kx022_write_byte( 0x18, 0x88 );   // 4G 
    
     kx022_read_byte( 0x18, &dummy );

    g_sensor_mode = EM_ACCEL_STATE_NORMAL;
    em_accel_int_enable();
}

static
void kx022_sleep_mode(void)
{
    ACCEL_SET_SLAVE_ADDR( KX022_SLAVE_ADDR );
    uint8_t dummy;
    
     kx022_write_byte( 0x18, 0x00 ); // CNTL1

     kx022_read_byte( 0x17, &dummy ); // INT_REL

     kx022_write_byte( 0x3E, 0xFF ); // BUF_CLEAR
     kx022_write_byte( 0x3B, 0x00 ); // BUF_CNTL2
     kx022_write_byte( 0x23, 0x03 ); // WUFC 
     kx022_write_byte( 0x30, 0x02 ); // ATH
     kx022_write_byte( 0x1D, 0x3F ); // INC2
     kx022_write_byte( 0x1F, 0x02 ); // INC4 
     kx022_write_byte( 0x1A, 0x04 ); // 50Hz 

     kx022_read_byte( 0x17, &dummy ); // INT REL
     kx022_write_byte( 0x18, 0x82 ); // CNTL1 
    
     kx022_read_byte( 0x18, &dummy );

    g_sensor_mode = EM_ACCEL_STATE_SLEEP;
    em_accel_int_enable();

    // axis_data_diff_count = 0;
    // axis_data_same_count = 0;
    // m_em_acc_is_enabled = true; 
    
//    accel_timer_start();
}

static
void kx022_fifo_mode(void)
{
    ACCEL_SET_SLAVE_ADDR( KX022_SLAVE_ADDR );
    em_accel_int_disable();

    uint8_t dummy;
    
     kx022_write_byte( 0x18, 0x00 );   // CNTL1

     kx022_read_byte( 0x17, &dummy );  // INT_REL
     kx022_write_byte( 0x1B, 0x02 );   // ODCNTL
     kx022_write_byte( 0x3E, 0xFF );   // BUF_CLEAR
     kx022_write_byte( 0x1D, 0x00 );   // INC2
     kx022_write_byte( 0x1A, 0x00 );   // CNTL3
     kx022_write_byte( 0x1F, 0x20 );   // INC4 
     kx022_write_byte( 0x3A, 0x0A );   // BUF_CNTL1
     kx022_write_byte( 0x3B, 0xC1 );   // BUF_CNTL2

     kx022_write_byte( 0x18, 0x88 );   // CNTL1 , 4G

     kx022_read_byte( 0x18, &dummy );
    
    g_sensor_mode = EM_ACCEL_STATE_FIFO;
    em_accel_int_enable();

    // m_em_acc_is_enabled = true; 
    accel_timer_restart( 3000 ); 
}

static
void kx022_stop_mode(void)
{
    ACCEL_SET_SLAVE_ADDR( KX022_SLAVE_ADDR );
    
    uint8_t dummy;
    
     kx022_read_byte( 0x17, &dummy );          // INT_REL
     kx022_write_byte( 0x18, 0x00 );   // CNTL1 
    
     kx022_read_byte( 0x18, &dummy );

    g_sensor_mode = EM_ACCEL_STATE_STOP;
    em_accel_int_disable();
}

static
void em_accel_set(uint8_t accel_status)
{
    switch( accel_status )
    {
        case EM_ACCEL_STATE_STOP:
        kx022_stop_mode();
        break;

        case EM_ACCEL_STATE_SLEEP:
        kx022_sleep_mode();
        break;
        
        case EM_ACCEL_STATE_FIFO:
        kx022_fifo_mode();
        break;

        case EM_ACCEL_STATE_NORMAL:
        kx022_normal_mode();
        break;

        case EM_ACCEL_STATE_TIMEOUT:
        break;
    }
}

static 
void kx022_timeout_handler(void)
{
    switch( g_sensor_mode )
    {
        case EM_ACCEL_STATE_FIFO:
        kx022_fifo_mode();
        break;

        case EM_ACCEL_STATE_SLEEP:
        break;
    }
}

const em_accel_instance_t kx022 = 
{
    .mounting = kx022_mounting,
    .setup = kx022_setup,
    .get_axes = kx022_read_axis,
    .get_fifo = kx022_get_fifo,
    // .sleep_check = em_accelerator_sleep_check,
    .set_status = em_accel_set
};

#endif // !MODULE_CHECK_ENABLED( ACCELERATOR ) && KX022_CONFIG_ENABLE
